/*
    Copyright 2006 Andrew Wilkinson <andrew@indiegigs.co.uk>

    This file is part of GL2x.

    GL2x is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    GL2x is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with GL2x; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include <math.h>

#include "gl_internal.h"

unsigned char Matrix_eq(Matrix this, Matrix other) {
    int x, y;

    for(y=3; y >= 0; y--) {
        for(x=3; x >= 0; x--) {
            if(Fixed_ne(element(this, x, y), element(other, x, y))) {
                return 0;
            }
        }
    }
    return 1;
}

Matrix Matrix_add(Matrix this, Matrix other) {
    int x, y;

    Matrix r;
    for(y=3; y >= 0; y--) {
        for(x=3; x >= 0; x--) {
            r.col[x][y] = Fixed_add(element(this, x, y), element(other, x, y));
        }
    }
    return r;
}

Matrix Matrix_sub(Matrix this, Matrix other) {
    int x, y;

    Matrix r;
    for(y=3; y >= 0; y--) {
        for(x=3; x >= 0; x--) {
            r.col[x][y] = Fixed_sub(element(this, x, y), element(other, x, y));
        }
    }
    return r;
}

Matrix Matrix_mul(Matrix this, Matrix other) {
    int x, y, i;

    Matrix r;
    for(y=3; y >= 0; y--) {
        for(x=3; x >= 0; x--) {
            r.col[x][y] = Fixed_zero;
            for(i=3; i >= 0; i--) {
                r.col[x][y] = Fixed_add(r.col[x][y], Fixed_mul(element(this, x, i), element(other, i, y)));
            }
        }
    }
    return r;
}

Vector Matrix_mulv(Matrix this, Vector other) {
    int x, i;

    Vector r;
    for(x=3; x >= 0; x--) {
        r.e[x] = Fixed_zero;
        for(i=3; i >= 0; i--) {
            r.e[x] = Fixed_add(r.e[x], Fixed_mul(element(this, x, i), other.e[i]));
        }
    }
    return r;
}

Matrix Matrix_identity() {
    Matrix r;
    Fixed one = Fixed_one;
    Fixed zer = Fixed_zero;
    r.col[0][0] = one; r.col[1][0] = zer; r.col[2][0] = zer; r.col[3][0] = zer;
    r.col[0][1] = zer; r.col[1][1] = one; r.col[2][1] = zer; r.col[3][1] = zer;
    r.col[0][2] = zer; r.col[1][2] = zer; r.col[2][2] = one; r.col[3][2] = zer;
    r.col[0][3] = zer; r.col[1][3] = zer; r.col[2][3] = zer; r.col[3][3] = one;
    return r;
}

Matrix Matrix_rotate(Fixed angle, Fixed x, Fixed y, Fixed z) {
    Vector v;
    Fixed c, nc, s;

    makevector(v, x, y, z, Fixed_one);
    v = Vector_normalize(v);
    x = v.x; y = v.y; z = v.z;

    c = Fixed_cos(angle);
    s = Fixed_sin(angle);

    nc = Fixed_sub(Fixed_one, c);

    Matrix r;
    r.col[0][0] = Fixed_add(Fixed_mul(Fixed_mul(x, x), nc), c);
    r.col[1][0] = Fixed_sub(Fixed_mul(Fixed_mul(x, y), nc), Fixed_mul(z, s));
    r.col[2][0] = Fixed_add(Fixed_mul(Fixed_mul(x, z), nc), Fixed_mul(y, s));
    r.col[3][0] = Fixed_zero;

    r.col[0][1] = Fixed_add(Fixed_mul(Fixed_mul(y, x), nc), Fixed_mul(z, s));
    r.col[1][1] = Fixed_add(Fixed_mul(Fixed_mul(y, y), nc), c);
    r.col[2][1] = Fixed_sub(Fixed_mul(Fixed_mul(y, z), nc), Fixed_mul(x, s));
    r.col[3][1] = Fixed_zero;

    r.col[0][2] = Fixed_sub(Fixed_mul(Fixed_mul(x, z), nc), Fixed_mul(y, s));
    r.col[1][2] = Fixed_add(Fixed_mul(Fixed_mul(y, z), nc), Fixed_mul(x, s));
    r.col[2][2] = Fixed_add(Fixed_mul(Fixed_mul(z, z), nc), c);
    r.col[3][2] = Fixed_zero;

    r.col[0][3] = Fixed_zero;
    r.col[1][3] = Fixed_zero;
    r.col[2][3] = Fixed_zero;
    r.col[3][3] = Fixed_one;

    return r;
}

Matrix Matrix_scale(Fixed x, Fixed y, Fixed z) {
    Matrix r;
    Fixed zer = Fixed_zero;
    r.col[0][0] = x;   r.col[1][0] = zer; r.col[2][0] = zer; r.col[3][0] = zer;
    r.col[0][1] = zer; r.col[1][1] = y;   r.col[2][1] = zer; r.col[3][1] = zer;
    r.col[0][2] = zer; r.col[1][2] = zer; r.col[2][2] = z;   r.col[3][2] = zer;
    r.col[0][3] = zer; r.col[1][3] = zer; r.col[2][3] = zer; r.col[3][3] = Fixed_one;
    return r;
}

Matrix Matrix_translate(Fixed x, Fixed y, Fixed z) {
    Matrix r;
    Fixed one = Fixed_one;
    Fixed zer = Fixed_zero;
    r.col[0][0] = one; r.col[1][0] = zer; r.col[2][0] = zer; r.col[3][0] = zer;
    r.col[0][1] = zer; r.col[1][1] = one; r.col[2][1] = zer; r.col[3][1] = zer;
    r.col[0][2] = zer; r.col[1][2] = zer; r.col[2][2] = one; r.col[3][2] = zer;
    r.col[0][3] = x;   r.col[1][3] = y;   r.col[2][3] = z;   r.col[3][3] = one;
    return r;
}
